from numpy import array,matrix,zeros,pi,linspace,log,floor
from constantes import perielioTerra,vPerielioTerra,mTerra
from matplotlib.pyplot import plot,show,subplot,title
from numpy.random import random
from numpy.linalg import norm

def Fx(px,py):
    return array([px/mTerra,py/mTerra]).reshape(2,1)

def Fp(x,y):
    temp1= (-x*4*pi**2*mTerra)/((x**2.0 + y**2.0)**(3.0/2.0))
    temp2= (-y*4*pi**2*mTerra)/((x**2.0 + y**2.0)**(3.0/2.0))
    return array([temp1,temp2]).reshape(2,1)

def Jacobiana(J,x,y):
    temp1=x**2.0+y**2.0
    temp2=temp1**(5.0/2.0)
    temp3=temp1**(3.0/2.0)
    temp4=3*(x**2.0)*4*pi**2*mTerra
    temp5=3*(y**2.0)*4*pi**2*mTerra
    temp6=3*x*y*4*pi**2*mTerra
    J[2,0]=((temp4)/(temp1))-((4*pi**2*mTerra)/(temp3))
    J[2,1]=(temp6)/(temp2)
    J[3,0]=(temp6)/(temp2)
    J[3,1]=((temp5)/(temp1))-((4*pi**2*mTerra)/(temp3))
    return J

#tempo final
tf=10**3

#tempo inicial
t0=0

n=19

T=2**n+1

t=linspace(t0,tf,T)
dt=t[1]-t[0]

x=zeros(T)
x[0]=perielioTerra
y=zeros(T)

px=zeros(T)

py=zeros(T)
py[0]=vPerielioTerra*mTerra;

J=matrix(zeros(16).reshape(4,4))
J[0,2]=1.0/mTerra
J[1,3]=1.0/mTerra

c=zeros(4)
d=zeros(4)
temp=(2-2**(1.0/3));
temp=1/temp;
c[0]=temp/2;
c[3]=c[0];
c[1]=temp/2;
c[1]=c[1]*(1-2**(1/3));
c[2]=c[1];
d[0]=temp;
d[2]=temp;
d[1]=temp;
d[1]=-d[1]*2**(1/3);
d[3]=0;

for i in range(1,T):
    
    dx=Fx(px[i-1],py[i-1])
    x[i]=x[i-1]+dt*c[0]*dx[0];
    y[i]=y[i-1]+dt*c[0]*dx[1];
       
    dx=Fp(x[i],y[i])
    px[i]=px[i-1]+dt*d[0]*dx[0];
    py[i]=py[i-1]+dt*d[0]*dx[1];
        
    dx=Fx(px[i],py[i])
    x[i]=x[i]+dt*c[1]*dx[0];
    y[i]=y[i]+dt*c[1]*dx[1];
       
    dx=Fp(x[i],y[i])
    px[i]=px[i]+dt*d[1]*dx[0];
    py[i]=py[i]+dt*d[1]*dx[1];
    
    dx=Fx(px[i],py[i])
    x[i]=x[i]+dt*c[2]*dx[0];
    y[i]=y[i]+dt*c[2]*dx[1];
       
    dx=Fp(x[i],y[i])
    px[i]=px[i]+dt*d[2]*dx[0];
    py[i]=py[i]+dt*d[2]*dx[1];
    
    dx=Fx(px[i],py[i])
    x[i]=x[i]+dt*c[3]*dx[0];
    y[i]=y[i]+dt*c[3]*dx[1];
       
    dx=Fp(x[i],y[i])
    px[i]=px[i]+dt*d[3]*dx[0];
    py[i]=py[i]+dt*d[3]*dx[1];
    
plot(x,y,'r')
show()
    
norma=[]
delta=random(4)*(10**-10)
delta=delta.reshape(4,1)

norma.append(norm(delta))
norma[0]=1;
delta=delta/norm(delta)
y2=zeros(T)
Y=zeros(T)

A=matrix('[ 1/18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 1/48, 1/16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 
          1/32, 0, 3/32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; 
          5/16, 0, -75/64, 75/64, 0, 0, 0, 0, 0, 0, 0, 0, 0; 
          3/80, 0, 0, 3/16, 3/20, 0, 0, 0, 0, 0, 0, 0, 0; 
          29443841/614563906, 0, 0, 77736538/692538347, -28693883/1125000000, 23124283/1800000000, 0, 0, 0, 0, 0, 0, 0;
          16016141/946692911, 0, 0, 61564180/158732637, 22789713/633445777, 545815736/2771057229, -180193667/1043307555, 0, 0, 0, 0, 0, 0;
          39632708/573591083, 0, 0, -433636366/683701615, -421739975/2616292301, 100302831/723423059, 790204164/839813087, 800635310/3783071287, 0, 0, 0, 0, 0;
          246121993/1340847787, 0, 0, -37695042795/15268766246, -309121744/1061227803, -12992083/490766935, 6005943493/2108947869, 393006217/1396673457, 123872331/1001029789, 0, 0, 0, 0;
         -1028468189/846180014, 0, 0, 8478235783/508512852, 1311729495/1432422823, -10304129995/1701304382, -48777925059/3047939560, 15336726248/1032824649, -45442868181/3398467696, 3065993473/597172653, 0, 0, 0;
          185892177/718116043, 0, 0, -3185094517/667107341, -477755414/1098053517, -703635378/230739211, 5731566787/1027545527, 5232866602/850066563, -4093664535/808688257, 3962137247/1805957418, 65686358/487910083, 0, 0;
          403863854/491063109, 0, 0, -5068492393/434740067, -411421997/543043805, 652783627/914296604, 11173962825/925320556, -13158990841/6184727034, 3936647629/1978049680, -160528059/685178525, 248638103/1413531060, 0, 0]');

fracao=0.15
normalizacao=floor(0.1*T)
for i in range(1,T):
    J=Jacobiana(J,x[i],y[i])
    delta=delta+dt*J*delta
    norma.append(norm(delta))
    y2[i]=((i-1)/(i))*y2[i-1]+2*log(norma[i]/norma[i-1])
    if i>fracao*T:
        Y[i]=((i-1)*Y[i-1]+y2[i])/i;
    
    if i%normalizacao==0:
        delta=delta/norm(delta)
        norma[i]=1;
        
    #delta=delta/norm(delta)
    norma[i]=1;
    
subplot(1,2,1)    
title('y')
plot(t,y2)
subplot(1,2,2)
title('Y')
plot(t,Y)
show()     
        